/*! \file swing1.cpp

\brief Swing up controller for Cartpole pendulum system

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.
*/
/*
INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param int ui: Real time

OUTPUT
\param double TAU: Torque input
\param VEC X: State derivate

*/

#include "../../dynlib.h"

void controller(Matrix &SimParam,Matrix &Xm,int i,int ui,double *U,ColumnVector &dX)
{
	//Declaracion de variables de tipo DOUBLE
	double signo, temp, Eo, M, I, w, E;

	//Declaracion de variables de tipo INT
	int fin, j;

	fin=(int)(SimParam(4,4)+SimParam(4,3));
	//Declaracion de matrices
	//Declaracion de vectores Columna
	ColumnVector X(fin);

	//Declaracion de vectores Fila

	for (j=1;j++;j<=fin){
	X(j)=Xm(j,i);}

	if(X(4)*cos(X(2))>0.0)
	{
	    signo=1.0;
	}
	else
	{
	    signo=-1.0;
	}
		
	temp=2;
	Eo=0;
	M=SimParam(1,1)+SimParam(1,2);
		
	I=(0.0833)*SimParam(1,2)*(SimParam(1,3)*SimParam(1,3))/2*7;
	w=sqrt((M*SimParam(1,4)*SimParam(1,3))/(4*I));
	E=M*SimParam(1,4)*SimParam(1,3)*(0.5*((X(4)/w)*(X(4)/w))+cos(X(2))-1);
	*U=(((SimParam(2,1)*SimParam(2,2)*(M))/(SimParam(2,3)*SimParam(2,4)))*(temp*(E-Eo)*signo+((SimParam(2,3)*SimParam(2,3)*SimParam(2,4)*SimParam(2,4)*X(3))/(SimParam(2,1)*SimParam(2,2)*SimParam(2,2)))))*1/7;

}
